package scu.maqiang.numeric;

import java.util.Arrays;
import java.util.function.Function;
import java.util.function.UnaryOperator;

import scu.maqiang.mesh.MatrixFunc;
import scu.maqiang.mesh.ScalarFunc;

public class NLESolver {
	public static double Bisection(Function<Double, Double> func, double a, double b, double tol) {
		double fa = func.apply(a);
		double fb = func.apply(b);
		double c = 0.0, fc = 0.0;
		if (fa * fb >= 0) {
			throw new IllegalArgumentException("f(a)f(b) < 0 not satisfied!");
		}
		while(0.5 * (b - a) > tol) {
			c = 0.5 * (a + b);
			fc = func.apply(c);
			if (Math.abs(fc) == 0.0) {
				break;
			}
			if (fa * fc < 0) {
				b = c;
				fb = fc;
			} else {
				a = c;
				fa = fc;
			}
		}
		
		return 0.5 * (a + b);
	}
	
	
	public static double FPI(Function<Double, Double> func, double x0, double tol, int maxIteration) {
		double x1 = func.apply(x0);
		int i = 1;
		while(Math.abs(x1 - x0) > tol) {
			System.out.println(i + "\t" + x1);
			x0 = x1;
			x1 = func.apply(x0);
			i++;
			if (i >= maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}
		return x1;
	}
	
	
	
	public static double Newton(Function<Double, Double> func, Function<Double, Double> df, double x0, double tol, int maxIteration) {
		double x1 = x0 - func.apply(x0)/df.apply(x0);
		int i = 1;
		while(Math.abs(x1 - x0) > tol) {
			System.out.println(i + "\t" + x1);
			x0 = x1;
			x1 = x0 - func.apply(x0)/df.apply(x0);
			i++;
			if (i >= maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}
		return x1;
	}
	
	
	
	public static double[] Newton(ScalarFunc[] equations, MatrixFunc jacobi, double[] x0, double tol, int maxIteration) {
		double[] x1 = x0.clone();
		int n = equations.length;
		double[] RHS = new double[n];
		for(int i = 0; i < n; i++) {
			RHS[i] = -equations[i].action(x0, 0, null);
		}
		double[][] x00 = new double[1][];
		x00[0] = x0;
		double[][] D = jacobi.action(x0, 0, null);
		DirectDSolver dsolver = new DirectDSolver(D);
		dsolver.PGaussSolve(RHS, x1);
		for(int i = 0; i < n; i++) {
			x1[i] += x0[i];
		}
		System.out.println(0 + "\t" + Arrays.toString(x0));
		int i = 1;
		while(MVO.L2Norm(MVO.add(x1, -1.0, x0)) > tol) {
			System.out.println(i + "\t" + Arrays.toString(x1));
			Arrays.setAll(x0, idx -> x1[idx]);
			for(int j = 0; j < n; j++) {
				RHS[j] = -equations[j].action(x0, 0, null);
			}
			x00[0] = x0;
			D = jacobi.action(x0, 0, null);
			dsolver.setMatrix(D);
			dsolver.PGaussSolve(RHS, x1);
//			System.out.println("min: " + MVO.min(x1) + "\tmax: " + MVO.max(x1));
			for(int j = 0; j < n; j++) {
				x1[j] += x0[j];
			}
			i++;
			if (i >= maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}
		
		return x1;
		
	}

	public static double[] ModifiedNewton(ScalarFunc[] equations, MatrixFunc jacobi, double[] x0, double tol, int maxIteration) {
		double[] x1 = x0.clone();
		int n = equations.length;
		double[] RHS = new double[n];
		for(int i = 0; i < n; i++) {
			RHS[i] = -equations[i].action(x0, 0, null);
		}
		double[][] x00 = new double[1][];
		x00[0] = x0;
		double[][] D = jacobi.action(x0, 0, null);
		DirectDSolver dsolver = new DirectDSolver(D);
		dsolver.PLU();
		dsolver.PLUSolve(RHS, x1);
		for(int i = 0; i < n; i++) {
			x1[i] += x0[i];
		}
		System.out.println(0 + "\t" + Arrays.toString(x0));
		int i = 1;
		while(MVO.L2Norm(MVO.add(x1, -1.0, x0)) > tol) {
			System.out.println(i + "\t" + Arrays.toString(x1));
			Arrays.setAll(x0, idx -> x1[idx]);
			for(int j = 0; j < n; j++) {
				RHS[j] = -equations[j].action(x0, 0, null);
			}
			x00[0] = x0;
			D = jacobi.action(x0, 0, null);
			dsolver.PLUSolve(RHS, x1);
//			System.out.println("min: " + MVO.min(x1) + "\tmax: " + MVO.max(x1));
			for(int j = 0; j < n; j++) {
				x1[j] += x0[j];
			}
			i++;
			if (i >= maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}

		return x1;

	}
	
	public static double[] BroydenI(ScalarFunc[] equations, double[][] A0, double[] x0, double tol, int maxIteration) {
		double[] x1 = x0.clone();
		double[] delta = x0.clone();
		double[] Delta = x0.clone();
		int n = equations.length;
		double[] RHS = new double[n];
		DirectDSolver dsolver = new DirectDSolver();
		int iterCount = 0;
		while(true) {
			Arrays.setAll(RHS, i -> -equations[i].action(x0, 0, null));
			dsolver.setMatrix(A0);
			dsolver.PGaussSolve(RHS.clone(), delta);
			if (MVO.L2Norm(delta) < Constants.Er) {
				break;
			}
			Arrays.setAll(x1, i -> x0[i] + delta[i]);
			Arrays.setAll(Delta, i -> equations[i].action(x1, 0, null) + RHS[i]);
			double[] ADelta = MVO.matmul(A0, delta);
			double delta2 = MVO.dot_product(delta, delta);
			for(int i = 0; i < n; i++) {
				for(int j = 0; j < n; j++) {
					A0[i][j] += (Delta[i] - ADelta[i]) * delta[j] / delta2;
				}
			}
			iterCount++;
			if (iterCount > maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}
		
		
		
		return x1;
	}
	
	public static double[] BroydenII(ScalarFunc[] equations, double[] x0, double tol, int maxIteration) {
		double[] x1 = x0.clone();
		return x1;
	}
	
	public static double Secant(Function<Double, Double> func, double x0, double x1, double tol, int maxIteration) {
		double x2 = x1 - func.apply(x1) * (x1 - x0)/(func.apply(x1) - func.apply(x0));
		System.out.println(1 + "\t" + x0);
		System.out.println(2 + "\t" + x1);
		System.out.println(3 + "\t" + x2);
		int i = 3;
		while(Math.abs(x2 - x1) > tol) {
			x0 = x1;
			x1 = x2;
			x2 = x1 - func.apply(x1) * (x1 - x0)/(func.apply(x1) - func.apply(x0));
			i++;
			System.out.println(i + "\t" + x2);
			if (i >= maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}
		return x2;
	}
	
	public static double RegulaFalsi(Function<Double, Double> func, double a, double b, double tol) {
		return 0.0;
	}
	
	
	public static void main(String[] args) {
//		Function<Double, Double> f = x -> x * x * x + x - 1;
//		Function<Double, Double> f2 = x -> Math.cos(x) - x;
//		Function<Double, Double> g1 = x -> 1 - x * x * x;
//		Function<Double, Double> g2 = x -> Math.pow(1 - x, 1.0/3.0);
//		Function<Double, Double> g3 = x -> (1 + 2 * x * x * x) / (1 + 3 * x * x);
//		UnaryOperator<Double> f3 = x -> x * x * x +  x - 1;
//		double x = Bisection(f2, 0, 1, 0.5e-6);
//		System.out.println(x);
//		
//		x = FPI(g1, 0.5, 0.5e-10, 100);
//		x = FPI(g2, 0.5, 0.5e-10, 100);
//		x = FPI(g3, 0.5, 0.5e-10, 100);
//		System.out.println(x);
//		
//		x = Secant(f, 0, 1, 1.0e-10, 100);
//		System.out.println(x);
		
//		Function<Double, Double> f =  x -> 1.0/x;
//		Function<Double, Double> df = x -> -1.0/(x * x);
//		double x = Newton(f, df, 1, 0.5e-10, 100);
		
		/*
		ScalarCoefFunc f1 = (xy, label, param) -> -xy[0] * xy[0] * xy[0] + xy[1];
		ScalarCoefFunc f2 = (xy, label, param) -> xy[0] * xy[0] + xy[1] * xy[1] - 1.0;
		
		MatrixCoefFunc DF = (xy, label, param) -> {
			double u = xy[0][0]; 
			double v = xy[0][1];
			double[][] D = {{-3 * u * u, 1}, {2 * u, 2 * v}};
			return D;
		};
		
		double[] x = Newton(new ScalarCoefFunc[] {f1, f2} , DF, new double[] {1.0, 2.0}, 0.5e-10, 100);
		*/

        /*
		ScalarFunc f1 = (xy, label, param) -> 2 * xy[0] * xy[0] - 4 * xy[0] + xy[1] * xy[1] + 3 * xy[2] * xy[2] + 6 * xy[2] + 2;
		ScalarFunc f2 = (xy, label, param) -> xy[0] * xy[0] + xy[1] * xy[1] - 2 * xy[1] + 2 * xy[2] * xy[2] - 5;
		ScalarFunc f3 = (xy, label, param) -> 3 * xy[0] * xy[0] - 12 * xy[0] + xy[1] * xy[1] + 3 * xy[2] * xy[2] + 8;
		
		MatrixFunc DF = (xy, label, param) -> {
			double u = xy[0];
			double v = xy[1];
			double w = xy[2];
			
			double[][] D = {{4 * u - 4, 2 * v, 6 * w + 6}, {2 * u, 2 * v - 2, 4 * w}, {6 * u - 12, 2 * v, 6 * w}};
			return D;
		};
		
		double[] x = Newton(new ScalarFunc[] {f1, f2, f3}, DF, new double[] {0.0, 0.0, 0.0}, 0.5e-10, 100);
		x = Newton(new ScalarFunc[] {f1, f2, f3}, DF, new double[] {10.0, 0.0, 0.0}, 0.5e-10, 100);
         */

		ScalarFunc f1 = (xy, label, param) -> 300 * xy[0] * xy[0] + 400 * xy[0] * xy[1] - 200 * xy[1] * xy[1] + 150 * xy[0] - 100 * xy[1];
		ScalarFunc f2 = (xy, label, param) -> 200 * xy[0] * xy[0] - 400 * xy[0] * xy[1] + 200 * xy[1] * xy[1] - 100 * xy[0] + 100 * xy[1] - 100;

		MatrixFunc DF = (xy, label, param) -> {
			double u1 = xy[0];
			double u2 = xy[1];
			double[][] D = {{600 * u1 + 400 * u2 + 150, 400 * (u1 - u2) - 100},{400 * (u1 - u2) - 100, 400 * (u2 - u1) + 100}};
			return D;
		};

		double[] x = ModifiedNewton(new ScalarFunc[] {f1, f2}, DF, new double[] {0.3, 0.6}, 0.5e-10, 100);

//		Function<Double, Double> f = u -> u + Math.atan(5 * u);
//		Function<Double, Double> df = u -> {
//			double temp = Math.cos(Math.atan(5 * u));
//			return 1 + 5 * temp * temp;
//		};
//
//		double x = Newton(f, df, 0.5, 1.0e-5, 20);

	}
}
